//********************************************/
//  mNCsetup                                **/
//  27.04.2011                              **/
//********************************************/
#include "mNC.h"
mNC::mNC()
{
    SetSolverV(V);
}
mNC::~mNC() {}

//LOADFILE
wxString mNC::LoadFile(wxString name)
{
    wxString filename;
    filename << PROGPATH << name;

    wxString str;
    wxTextFile file;
    if(!file.Open(filename))
        return _("err:");

    for(size_t a=0; a<file.GetLineCount(); a++)
    {
        str << file.GetLine(a) << _("\n");
    }
    str.Trim(true);
    return str;
}
//**************************//
void mNC::CYCLE(wxString code)
{
    I.clear();
    ERR = _("");
    MSG = _("");

    //clear
    for(uint a=0; a<5200; a++)
    {
        V[a]=0;
    }
    //clear
    V[Group2]=17;
    V[Group9]=80;
    V[Group14]=54;

    G0(0,0,0);

    //DEFAULT
    V[TLOV_X] = V[G54_X];
    V[TLOV_Y] = V[G54_Y];
    V[TLOV_Z] = V[G54_Z];

    MSG.Clear();
    Run(code);

    //SMALLER
    for(uint a=0; a<I.size(); a++)
    {
        I.at(a).x = I.at(a).x - V[G54_X];
        I.at(a).y = I.at(a).y - V[G54_Y];
        I.at(a).z = I.at(a).z - V[G54_Z];
    }
}

//************************//
//**
//**
//**
//************************//
void mNC::Run(wxString code)
{
    int LOOPLIMIT=1000000;       //maximum loop break
    size_t rp=-1;                //row number

    //return if it has small letters
    for(size_t a=0; a<code.Len(); a++)
    {
        if(code[a]>93)
        {
            MSG << wxString::Format( _("\n IS NOT G code\n"));
            return;
        }
    }



    if(code.Contains(_("err::")))return;

    //******************************************************************//
    wxArrayString rows = wxStringTokenize(code, wxT("\n"),wxTOKEN_STRTOK);  //parse rows

    while(true)
    {
        rp++;
        if(rows.GetCount()<=rp)break;
        if(!ERR.Matches(_("")))break;

        Parse(rows.Item(rp));

        //(COMMENT REMOVE) AlWays to the end of the row....
        for(uint a=0; P.GetCount()>a; a++)
            if(P[a].IsSameAs(_("(")))
                P.RemoveAt(a,P.GetCount()-a);
        if(P.GetCount()==0)continue;

        EVA();

        if(P.GetCount()==0)continue;

        //END ROUTINES
        //WHILE
        if(P[0].IsSameAs(_("WHILE")) && IsConstant(1) && P.GetCount()==4)
        {
            if( P[1].IsSameAs(_("1.0000000000")))continue;
            else
            {
                wxString dta;
                dta = wxString::Format(_("END%s"),P[3].c_str());

                while(true)
                {
                    if(rp==rows.GetCount())
                    {
                        ERR << _("CAN'T FIND END BLOCK");
                        break;
                    }
                    if(rows.Item(rp).Contains(dta))break;
                    rp++;
                }
                continue;
            }
        }

        if(P[0].IsSameAs(_("END"))  && IsConstant(1) && P.GetCount()==2 )
        {
            wxString dta = wxString::Format(_("DO%s"),P[1].c_str());

            while(true)
            {
                rp--;
                if(rp<1)
                {
                    ERR << _("CAN'T FIND DO BLOCK");
                    break;
                }
                if(rows.Item(rp).Contains(dta))
                {
                    rp--;
                    break;
                }
            }
            continue;
        }


        if(P[0].IsSameAs(_("IF"))  && IsConstant(1) && P[2].IsSameAs(_("THEN")) )
        {

            if(P[1].IsSameAs(_("1.0000000000")))
            {
                P.RemoveAt(0,3);
                EVA();
                continue;

            }
            else
            {
                continue;
            }
        }

        if(P[0].IsSameAs(_("IF"))  && IsConstant(1) && P[2].IsSameAs(_("GOTO")) )
        {
            if( P[1].IsSameAs(_("1.0000000000")))
            {
                P.RemoveAt(0,2);
            }
            else
            {
                continue;
            }
        }


        if(P[0].IsSameAs(_("GOTO")) && IsConstant(1))
        {
            wxString dta = wxString::Format(_("N%1.0f"), ToDouble(P[1]));

            rp=0;
            while(true)
            {
                if(rp==rows.GetCount())
                {
                    ERR << _("CAN'T FIND DO BLOCK NUMBER");
                    break;
                }
                if(rows.Item(rp).Contains(dta))break;

                rp++;
            }

            continue;
        }

        //************************//
        //       ResetKeys        //
        //************************//
        for(int a=4101; a<4127; a++)
        {
            V[a] = NAN;
        }
        //***********//
        for(uint a=0; P.GetCount()>a; a++)
        {
            if(P[a].IsSameAs(_("%")))
                continue;


            //MODAL.....
            double G;

            if( P[a].Len()>1)   //should not bee over one char
            {
                ERR << _("ERROR ") << P[a];
            }

            if(P[a].IsSameAs(_("G")))
            {
                a++;
                G=ToDouble( P[a] );

                if(G==0 || G==1 || G==2 || G==3 || G==33)V[Group1]=G;       //Group1
                else if(G==17 || G==18 || G==19)V[Group2]=G;                     //Group2
                else if(G==90 || G==91)V[Group3]=G;                              //Group3
                //Group4
                else if(G==94 || G==95)V[Group5]=G;                              //Group5 G94,G95
                else if(G==20 || G==21)V[Group6]=G;                              //Group6 G20,G21
                else if(G==40 || G==41 || G==42)V[Group7]=G;                     //Group7 G40,G41,G42
                else if(G==43 || G==44 || G==49)V[Group8]=G;                     //Group8 G43,G44,G49
                else if((G==73 || G==74 || G==76) || (G>79 && G<90))V[Group9]=G; //Group9 G73,G74,G76,G80-G89
                else if(G==98 || G==99)V[Group10]=G;                             //Group10 G98,G99
                else if(G==50 || G==51)V[Group11]=G;                             //Group11 G50,G51
                else if(G==66 || G==67)V[Group12]=G;                             //Group12 G66,G67
                else if(G==69 || G==97)V[Group13]=G;                             //Group13 G69,G97
                else if(G>53 && G<60)V[Group14]=G;                               //Group14 G54-G59
                else if(G>60 && G<65)V[Group15]=G;                               //Group15 G61-G64
                else if(G==68 || G==69)V[Group16]=G;                             //Group16 G68,G69
                else
                {
                    V[Gcode]=G;
                }
                continue;
            };

            //THESE SHOULD BE ONE TIMERS
            if(P[a].IsSameAs(_("A")))
            {
                a++;
                V[Acode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("B")))
            {
                a++;
                V[Bcode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("C")))
            {
                a++;
                V[Ccode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("D")))
            {
                a++;
                V[Dcode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("E")))
            {
                a++;
                V[Ecode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("F")))
            {
                a++;
                V[Fcode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("H")))
            {
                a++;
                V[Hcode] = ToDouble( P[a] );
                continue;
            }


            //DOT
            if(P[a].IsSameAs(_("I")))
            {
                double kerroin=0.001;
                a++;
                if( P[a].Matches(_("*.*")))kerroin=1;
                V[Icode] = ToDouble( P[a] )*kerroin;
                continue;
            }
            if(P[a].IsSameAs(_("J")))
            {
                double kerroin=0.001;
                a++;
                if( P[a].Matches(_("*.*")))kerroin=1;
                V[Jcode] = ToDouble( P[a] )*kerroin;
                continue;
            }
            if(P[a].IsSameAs(_("K")))
            {
                double kerroin=0.001;
                a++;
                if( P[a].Matches(_("*.*")))kerroin=1;
                V[Kcode] = ToDouble( P[a] )*kerroin;
                continue;
            }

            if(P[a].IsSameAs(_("L")))
            {
                a++;
                V[Lcode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("M")))
            {
                a++;
                V[Mcode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("N")))
            {
                a++;
                V[Ncode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("O")))
            {
                a++;
                V[Ocode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("P")))
            {
                a++;
                V[Pcode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("Q")))
            {
                a++;
                V[Qcode] = ToDouble( P[a] );
                continue;
            }

            /******************/
            if(P[a].IsSameAs(_("R")))
            {
                double kerroin=0.001;
                a++;
                if( P[a].Matches(_("*.*")))kerroin=1;
                V[Rcode] = ToDouble( P[a] )*kerroin;
                continue;
            }


            /******************/

            if(P[a].IsSameAs(_("S")))
            {
                a++;
                V[Scode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("T")))
            {
                a++;
                V[Tcode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("U")))
            {
                a++;
                V[Ucode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("V")))
            {
                a++;
                V[Vcode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("W")))
            {
                a++;
                V[Wcode] = ToDouble( P[a] );
                continue;
            }
            if(P[a].IsSameAs(_("X")))
            {
                double kerroin=0.001;
                a++;
                if( P[a].Matches(_("*.*")))kerroin=1;
                V[Xcode] = ToDouble( P[a] )*kerroin;
                continue;
            }
            if(P[a].IsSameAs(_("Y")))
            {
                double kerroin=0.001;
                a++;
                if( P[a].Matches(_("*.*")))kerroin=1;
                V[Ycode] = ToDouble( P[a] )*kerroin;
                continue;
            }
            if(P[a].IsSameAs(_("Z")))
            {
                double kerroin=0.001;
                a++;
                if( P[a].Matches(_("*.*")))kerroin=1;
                V[Zcode] = ToDouble( P[a] )*kerroin;

                continue;
            }


            ERR << _("ERROR WRONG CHARACTER") << P[a];
            break;
        };

        //***************************************************//
        //*****             EXECUTE BLOCK    ****************//
        //***************************************************//
        if(!ERR.Matches(_("")))
            break;
        //G43 -- ToolOffest --
        if(int(V[Group8])==43)
        {
            V[Group8]=0;
            continue;
        }

        //G43 -- ROTATE MATRIX --
        if(int(V[Group16])==68)
        {
            V[Group16]=1;                     //TRANSFORM LOCATION TO CORRECT PLACE
            V[ROTATE_ANGLE] = V[Rcode]*DegToRad;

            V[ROTATE_X] = V[Xcode];
            V[ROTATE_Y] = V[Ycode];
            V[ROTATE_Z] = V[Zcode];

            continue;
        }

        if(int(V[Group16])==69)
        {
            continue;
        }


        //G92 -- CHANCE --
        if(int(V[Gcode])==92)
        {
            V[Gcode]=NAN;
            V[TLOV_X] = V[int(G54_X + ((V[Group14]-54)*20))];
            V[TLOV_Y] = V[int(G54_Y + ((V[Group14]-54)*20))];
            V[TLOV_Z] = V[int(G54_Z + ((V[Group14]-54)*20))];
            continue;
        }
        if(int(V[Gcode])==28)
        {
            V[Gcode]=NAN;
            V[TLOV_X] = 0;
            V[TLOV_Y] = 0;
            V[TLOV_Z] = 0;
        }

        //MAKROCALL G65
        if(int(V[Gcode])==65)
        {
            SubRun( LoadFile(wxString::Format(wxT("%1.0f"),V[Pcode])) );
            continue;
        }

        if(int(V[Mcode])==30)
        {
            MSG << _("normal exit\n");
            break;
        }
        if(int(V[Mcode])==99)
        {
            MSG << _("");
            break;
        }

        //******************//
        if(int(V[Gcode])==700)
        {
            SubRun(LoadFile(_("9010")));
            continue;
        }
        if(int(V[Gcode])==701)
        {
            SubRun(LoadFile(_("9011")));
            continue;
        }
        if(int(V[Gcode])==702)
        {
            SubRun(LoadFile(_("9012")));
            continue;
        }
        if(int(V[Gcode])==703)
        {
            SubRun(LoadFile(_("9013")));
            continue;
        }
        if(int(V[Gcode])==704)
        {
            SubRun(LoadFile(_("9014")));
            continue;
        }

        if(int(V[Gcode])==705)
        {
            SubRun(LoadFile(_("9015")));
            continue;
        }

        //******************//
        if(int(V[Mcode])==150)
        {
            SubRun(LoadFile(_("9001")));
            continue;
        }

        //******************//
        if(int(V[Mcode])==100)
        {
            SubRun(LoadFile(_("9020")));
            continue;
        }
        if(int(V[Mcode])==101)
        {
            SubRun(LoadFile(_("9021")));
            continue;
        }
        if(int(V[Mcode])==102)
        {
            SubRun(LoadFile(_("9022")));
            continue;
        }
        if(int(V[Mcode])==200)
        {
            SubRun(LoadFile(_("9023")));
            continue;
        }
        if(int(V[Mcode])==201)
        {
            SubRun(LoadFile(_("9024")));
            continue;
        }
        if(int(V[Mcode])==202)
        {
            SubRun(LoadFile(_("9025")));
            continue;
        }
        //*************//
        //CANNED CYCLES//
        //*************//
        if(V[Group9]==76)
        {
            if(!isnan(V[Kcode])) V[Kcanned] = V[Kcode];
            if(!isnan(V[Rcode])) V[Rcanned] = V[Rcode];
            if(!isnan(V[Zcode])) V[Zcanned] = V[Zcode];
            if(isnan(V[Ycode])&&isnan(V[Xcode]))continue;  //JUMP OVER IF NO CHANGE
            if(V[Kcanned]<1)V[Kcanned]=1;

            for(int a=0; V[Kcanned]>a; a++)
            {
                G0(V[Xcode],V[Ycode],NAN);
                G0(NAN,NAN, V[Rcanned] );
                G1(NAN,NAN, V[Zcanned] );
                G0(NAN,NAN, V[Rcanned] );
            }
            V[Kcanned]=1;
            continue;
        }

        //**************//
        //DRILLING CYCLE//
        //**************//
        if(V[Group9]==81)
        {
            if(!isnan(V[Kcode])) V[Kcanned] = V[Kcode];
            if(!isnan(V[Rcode])) V[Rcanned] = V[Rcode];
            if(!isnan(V[Zcode])) V[Zcanned] = V[Zcode];
            if(isnan(V[Ycode])&&isnan(V[Xcode]))continue;  //JUMP OVER IF NO CHANGE
            if(V[Kcanned]<1) V[Kcanned]=1;

            for(int a=0; V[Kcanned]>a; a++)
            {
                G0(V[Xcode],V[Ycode],NAN);
                G0(NAN,NAN, V[Rcanned] );
                G1(NAN,NAN, V[Zcanned] );
                G0(NAN,NAN, V[Rcanned] );
            }
            V[Kcanned]=1;
            continue;
        }
        //*************//
        //TAPPING CYCLE//
        //*************//
        if(V[Group9]==84)
        {
            if(!isnan(V[Kcode])) V[Kcanned] = V[Kcode];
            if(!isnan(V[Rcode])) V[Rcanned] = V[Rcode];
            if(!isnan(V[Zcode])) V[Zcanned] = V[Zcode];
            if(isnan(V[Ycode])&&isnan(V[Xcode]))continue;  //JUMP OVER IF NO CHANGE
            if(V[Kcanned]<1)V[Kcanned]=1;

            for(int a=0; V[Kcanned]>a; a++)
            {
                G0(V[Xcode],V[Ycode],NAN);
                G0(NAN,NAN, V[Rcanned] );
                G1(NAN,NAN, V[Zcanned] );
                G1(NAN,NAN, V[Rcanned] );
            }
            V[Kcanned]=1;
            continue;
        }
        //*************//
        //**         **//
        //*************//
        if(!isnan(V[Fcode])) V[CurrentFeed]=V[Fcode];
        if(!isnan(V[Tcode])) V[CurrentTool]=V[Tcode];

        if(V[Group9]==80)
        {
            if(!isnan(V[Rcanned])) V[Rcanned] = NAN;
            if(!isnan(V[Kcanned])) V[Kcanned] = NAN;
            if(!isnan(V[Zcanned])) V[Zcanned] = NAN;

            if( (!isnan(V[Xcode])) ||  (!isnan(V[Ycode])) ||  (!isnan(V[Zcode])) || (!isnan(V[Icode])) ||  (!isnan(V[Jcode])) ||  (!isnan(V[Kcode])) )
            {
                if(!isnan(V[Xcode]))V[CP_X] = V[Xcode];
                if(!isnan(V[Ycode]))V[CP_Y] = V[Ycode];
                if(!isnan(V[Zcode]))V[CP_Z] = V[Zcode] ;

                //execute ROTATE location manipulation experimental.........
                if(int(V[Group16])==1)
                {
                    if(isnan(V[Xcode]))V[Xcode]=V[CP_X];
                    if(isnan(V[Ycode]))V[Ycode]=V[CP_Y];
                    if(isnan(V[Zcode]))V[Zcode]=V[CP_Z];

                    mPoint T;
                    T.SetValue(mP(V[Xcode],V[Ycode],V[Zcode]));

                    //T -= mP(V[ROTATE_X],V[ROTATE_Y],V[ROTATE_Z]);
                    T.RotateXY(V[ROTATE_ANGLE]);
                    //T += mP(V[ROTATE_X],V[ROTATE_Y],V[ROTATE_Z]);
                    V[Xcode] = T.x;
                    V[Ycode] = T.y;
                    V[Zcode] = T.z;
                }

                if((V[Group1]==2 || V[Group1]==3) && (!isnan(V[Rcode])))
                {
                    if(isnan(V[Xcode]))V[Xcode]=V[CPM_X]-V[TLOV_X];
                    if(isnan(V[Ycode]))V[Ycode]=V[CPM_Y]-V[TLOV_Y];
                    if(isnan(V[Zcode]))V[Zcode]=V[CPM_Z]-V[TLOV_Z];

                    mPoint p1,p2,cp;
                    p1.SetValue(V[CPM_X]-V[TLOV_X],V[CPM_Y] -V[TLOV_Y],V[CPM_Z]-V[TLOV_Z]);
                    p2.SetValue(V[Xcode],V[Ycode],V[Zcode]);
                    if(V[Group1]==2)cp.SetCWCircleCenter(p1,p2,V[Rcode]);
                    if(V[Group1]==3)cp.SetCCWCircleCenter(p1,p2,V[Rcode]);
                    V[Icode]=cp.x-p1.x;   //CENTER POSITION....
                    V[Jcode]=cp.y-p1.y;
                    V[Kcode]=cp.z-p1.z;
                }

                if(V[Group1]==0)G0(V[Xcode],V[Ycode],V[Zcode]);
                if(V[Group1]==1)G1(V[Xcode],V[Ycode],V[Zcode]);
                if(V[Group1]==2)G2(V[Xcode],V[Ycode],V[Zcode],V[Icode],V[Jcode],V[Kcode]);
                if(V[Group1]==3)G3(V[Xcode],V[Ycode],V[Zcode],V[Icode],V[Jcode],V[Kcode]);
            }


        }
        LOOPLIMIT--;
        if(LOOPLIMIT==0)ERR << _("LOOPLIMIT");
    };
    MSG = wxString::Format( _("\ncycle info:\nRunTime=%1.0fmins%1.3fsecs\n "),floor(V[RunTime]),(V[RunTime]-floor(V[RunTime]))*60);
}
//
void mNC::SubRun(wxString code)
{
    V[Gcode]=NAN;
    //SACE LOCAL VARIABLES
    double BUF[35];
    for(int a=0; a<35; a++)
    {
        BUF[a]=V[a];
    }

    //SET VARIABLES
    V[1]=V[Acode];
    V[2]=V[Bcode];
    V[3]=V[Ccode];
    V[7]=V[Dcode];
    V[8]=V[Ecode];
    V[9]=V[Fcode];
    V[11]=V[Hcode];

    V[4]=V[Icode];
    V[5]=V[Jcode];
    V[6]=V[Kcode];
    V[13]=V[Mcode];
    V[17]=V[Qcode];
    V[18]=V[Rcode];
    V[19]=V[Scode];

    V[20]=V[Tcode];
    V[21]=V[Ucode];
    V[22]=V[Vcode];
    V[23]=V[Wcode];
    V[24]=V[Xcode];
    V[25]=V[Ycode];
    V[26]=V[Zcode];
    //SET VARIABLESS BACK
    Run(code);
    for(int a=0; a<35; a++)
    {
        V[a]=BUF[a];
    }
}



//G1
void mNC::G0(double X,double Y,double Z)
{
    if(isnan(X)) X = V[CPM_X];
    else X=X+V[TLOV_X];
    if(isnan(Y)) Y = V[CPM_Y];
    else Y=Y+V[TLOV_Y];
    if(isnan(Z)) Z = V[CPM_Z];
    else Z=Z+V[TLOV_Z];

    V[RunTime] += (Distance3D(X,Y,Z,V[CPM_X],V[CPM_Y],V[CPM_Z]))/12000;

    addP( mP(X,Y,Z) , 0 ,V[RunTime] , V[CurrentTool] ) ;

    V[CPM_X]=X;
    V[CPM_Y]=Y;
    V[CPM_Z]=Z;
}

//G1
void mNC::G1(double X,double Y,double Z)
{
    if(isnan(X)) X = V[CPM_X];
    else X=X+V[TLOV_X];
    if(isnan(Y)) Y = V[CPM_Y];
    else Y=Y+V[TLOV_Y];
    if(isnan(Z)) Z = V[CPM_Z];
    else Z=Z+V[TLOV_Z];

    //printf("feed%f\n",V[CurrentFeed]);


    V[RunTime] += (Distance3D(X,Y,Z,V[CPM_X],V[CPM_Y],V[CPM_Z]))/V[CurrentFeed];

    addP( mP(X,Y,Z) , 1 ,V[RunTime] , V[CurrentTool]  );


    V[CPM_X]=X;
    V[CPM_Y]=Y;
    V[CPM_Z]=Z;
}

//G2
void mNC::G2(double X,double Y,double Z,double CX,double CY,double CZ)
{
    if(isnan(X)) X = V[CPM_X];
    else X=X+V[TLOV_X];
    if(isnan(Y)) Y = V[CPM_Y];
    else Y=Y+V[TLOV_Y];
    if(isnan(Z)) Z = V[CPM_Z];
    else Z=Z+V[TLOV_Z];

    if(isnan(CX)) CX = 0;
    if(isnan(CY)) CY = 0;
    if(isnan(CZ)) CZ = 0;

    mPoint P1,P2,CP;
    P2.SetValue(X,Y,Z);
    P1.SetValue(V[CPM_X],V[CPM_Y],V[CPM_Z]);
    CP.SetValue(P1.x+CX,P1.y+CY,P1.z+CZ);

    double a1 = CP.GetDirectionXY(P1);
    double a2 = CP.GetDirectionXY(P2);
    double  r = CP.GetDistanceXY(P1);

    if(!AER(r,CP.GetDistanceXY(P2),0.001))
    {
        printf("RADIUS ERROR G2 i=%f J=%f K=%f %f!=%f\n",CX,CY,CZ,r,CP.GetDistanceXY(P2));
    }

    double arc= 0;

    if(a1<=a2) a2-=TAU;

    double drop;

    if(a2<0)
        arc = a1+(-a2);
    else
        arc = a1-a2;

    V[RunTime] += (arc*r)/V[CurrentFeed];;
    drop = (V[CPM_Z]-Z)/arc*0.1;

    //LOOP
    while(a1>a2)
    {
        a1=a1-0.1;
        V[CPM_Z] -= drop;
        if(a1<a2)a1=a2;


        addP(mP(cos(a1)*r+CP.x,sin(a1)*r+CP.y,V[CPM_Z]) , 2 ,

                        I.at(I.size()-1).ny+(Distance3D(I.at(I.size()-1).x ,

                                               I.at(I.size()-1).y,
                                               I.at(I.size()-1).z,
                                               cos(a1)*r+CP.x,
                                               sin(a1)*r+CP.y,
                                               V[CPM_Z]))/V[CurrentFeed]

                        , V[CurrentTool] ) ;
    }
    V[CPM_X]=X;
    V[CPM_Y]=Y;
    V[CPM_Z]=Z;
}

//G3
void mNC::G3(double X,double Y,double Z,double CX,double CY,double CZ)
{
    if(isnan(X)) X = V[CPM_X];
    else X=X+V[TLOV_X];
    if(isnan(Y)) Y = V[CPM_Y];
    else Y=Y+V[TLOV_Y];
    if(isnan(Z)) Z = V[CPM_Z];
    else Z=Z+V[TLOV_Z];

    if(isnan(CX)) CX = 0;
    if(isnan(CY)) CY = 0;
    if(isnan(CZ)) CZ = 0;

    mPoint P1,P2,CP;
    P2.SetValue(X,Y,Z);
    P1.SetValue(V[CPM_X],V[CPM_Y],V[CPM_Z]);
    CP.SetValue(P1.x+CX,P1.y+CY,P1.z+CZ);

    double a1 = CP.GetDirectionXY(P1);
    double a2 = CP.GetDirectionXY(P2);

    double  r = CP.GetDistanceXY(P1);
    if(!AER(r,CP.GetDistanceXY(P2),0.001))
    {
        printf("RADIUS ERROR G3 i=%f J=%f K=%f %f!=%f\n",CX,CY,CZ,r,CP.GetDistanceXY(P2));
    };
    if(a1>=a2)a2+=TAU;

    V[RunTime] += ((a2-a1)*r)/V[CurrentFeed];;
    double drop = (V[CPM_Z]-Z)/((a2-a1))*0.1;

    while(a1<a2)
    {
        a1=a1+0.1;
        V[CPM_Z] -= drop;
        if(a1>a2)a1=a2;

        addP(mP( cos(a1)*r+CP.x,sin(a1)*r+CP.y ,V[CPM_Z]) , 3 ,
                        I.at(I.size()-1).ny+(Distance3D(I.at(I.size()-1).x ,
                        I.at(I.size()-1).y,I.at(I.size()-1).z,
                        cos(a1)*r+CP.x,sin(a1)*r+CP.y,
                        V[CPM_Z]))/V[CurrentFeed],V[CurrentTool]
                    );
    }
    V[CPM_X]=X;
    V[CPM_Y]=Y;
    V[CPM_Z]=Z;
}

//*******************************//
//**
//*******************************//
bool mNC::GetPos()
{
    pos.SetValue(-V[G54_X]*0.001,-V[G54_Y]*0.001,-V[G54_Z]*0.001);

    uint a=0;

    for(a=0; a<I.size(); a++)
        if(aika<I.at(a).ny)break;

    double f = I.at(a).ny - I.at(a-1).ny;
    double p = I.at(a).ny -aika;
    double x = ((I.at(a-1).x - I.at(a).x)/f*p)+I.at(a).x;
    double y = ((I.at(a-1).y - I.at(a).y)/f*p)+I.at(a).y;
    double z = ((I.at(a-1).z - I.at(a).z)/f*p)+I.at(a).z;

    pos.SetValue(x,y,z);                //Set position
    tool = int(I.at(a).nz);           //Set Tool Number
    feed = f;
    return true;
}

//*****************************//
//Gets Position
bool mNC::SetTime(double setaika)
{
    aika = setaika;
    if(aika<0)
    {
        aika=0;
        GetPos();
        return false;
    }

    if(aika>GetCycleTime())
    {
        aika=GetCycleTime();
        GetPos();
        return false;
    }
    GetPos();
    return true;
}

//sekunnin 25 osa
bool mNC::Tick(double add)
{
    aika=aika+add;
    if(aika<0)
    {
        aika=0;
        GetPos();
        return false;
    }

    if(aika>GetCycleTime())
    {
        aika=GetCycleTime()-0.0000001;
        GetPos();
        return false;
    }
    GetPos();
    return true;
}
//info
double mNC::GetCycleTime()
{
    if(I.size()==0)return 0;
    return I.at(I.size()-1).ny;;
}


//****************************//
void mNC::ToV(int a,double luku)
{
    V[a]=luku;
    return;
}

//************************************************//
double mNC::GetV(int a)
{
    return V[a];
}
//*************************************************//
void mNC::addP(mPoint p,uint G,double Aika,double Tool)
{
    vertex ulos;
    ulos.x = p.x;
    ulos.y = p.y;
    ulos.z = p.z;

    if(G==0)
    {
        ulos.r = 1;
        ulos.g = 0;
        ulos.b = 0;
        ulos.a = 1;
    }
    else
    {
        ulos.r = 0;
        ulos.g = 1;
        ulos.b = 0;
        ulos.a = 1;
    }

    ulos.nx = (float)G;
    ulos.ny = Aika;
    ulos.nz = Tool;
    if(I.size()>0)
    {
        if((uint)I[I.size()-1].nx != G)
        {
            I.push_back( I[I.size()-1] );
            I[I.size()-1].r = ulos.r;
            I[I.size()-1].g = ulos.g;
            I[I.size()-1].b = ulos.b;
        }
    }
    I.push_back( ulos );
}

